#include <kernel.h>

#define IPS_NODE_SIZE 12

static char __ips_size_assert[sizeof(ips_node_t) == IPS_NODE_SIZE ? 0 : -1];
static char __mutex_size_assert[sizeof(mutex_t) == 8 ? 0 : -1];
static char __sem_size_assert[sizeof(semaphore_t) == 12 ? 0 : -1];
static char __bq_size_assert[sizeof(bqueue_t) == 8 ? 0 : -1];

void
ips_wait(ips_node_t *node)
{
	 proc_legacy_wait_pretend();
	 while (1)
	 {
		  if (!node->wait || !node->ac_wait)
			   break;
		  proc_legacy_wait_try();
	 }
}

int
ips_wait_try(ips_node_t *node)
{
	 return node->wait && node->ac_wait;
}

int
ips_is_finish(ips_node_t *node)
{
	 return !node->wait;
}

#define EVENT_TO_ID(node) ((uintptr_t)(node) >> 2)
#define ID_TO_EVENT(node) ((proc_event_t)((uintptr_t)(node) << 2))

#define NODE_PTR_TO_ID(node) ((uintptr_t)(node) >> 2)
#define NODE_ID_TO_PTR(node) ((ips_node_t *)((uintptr_t)(node) << 2))

void
mutex_init(mutex_t *mutex)
{
	 spl_init(&mutex->lock);
	 mutex->hold = 0;
}

int
mutex_try_acquire(mutex_t *mutex)
{
	 int result;

	 intr_irq_save();
	 spl_acquire(&mutex->lock);
	 if (mutex->hold == 0)
	 {
		  mutex->hold = 1;
		  mutex->wait = 0;
		  
		  result = 0;
	 }
	 else result = 1;
	 spl_release(&mutex->lock);
	 intr_irq_restore();
	 
	 return result;
}

int
mutex_acquire(mutex_t *mutex, ips_node_t *node, proc_event_t event)
{
	 if (node == NULL)
	 {
		  ips_node_t node;
		  if (mutex_acquire(mutex, &node, event))
			   ips_wait(&node);

		  return 0;
	 }
	 else
	 {
		  intr_irq_save();
		  spl_acquire(&mutex->lock);
		  
		  if (mutex->hold == 0)
		  {
			   mutex->hold = 1;
			   mutex->wait = 0;
			   
			   spl_release(&mutex->lock);
			   intr_irq_restore();
			   
			   node->wait = 0;
			   
			   return 0;
		  }
		  else
		  {
			   node->event   = EVENT_TO_ID(event == NULL ? proc_self_dummy_event() : event);
			   node->ac_wait = 1;
			   node->wait    = 1;
			   
			   if (mutex->wait)
			   {
					node->next = NODE_ID_TO_PTR(mutex->head);
					node->prev = node->next->prev;
					node->next->prev = node;
					node->prev->next = node;
			   }
			   else
			   {
					mutex->wait = 1;
					node->next = node->prev = node;
					mutex->head = NODE_PTR_TO_ID(node);
			   }
			   
			   spl_release(&mutex->lock);
			   intr_irq_restore();
			   
			   return 1;
		  }
	 }
}

void
mutex_ac_break(mutex_t *mutex, ips_node_t *node)
{
	 intr_irq_save();
	 spl_acquire(&mutex->lock);
	 if (node->wait)
	 {
		  node->next->prev = node->next;
		  node->prev->next = node->prev;
		  
		  if (mutex->head == NODE_PTR_TO_ID(node))
		  {
			   if (node->next == node)
					mutex->wait = 0;
			   else mutex->head = NODE_PTR_TO_ID(node->next);
		  }
	 }
	 spl_release(&mutex->lock);
	 intr_irq_restore();
	 
	 node->ac_wait = 0;
}

void
mutex_release(mutex_t *mutex)
{
	 proc_event_t notify = NULL;
	 if (!mutex->hold) return;
	 intr_irq_save();
	 spl_acquire(&mutex->lock);
	 if (mutex->wait)
	 {
		  ips_node_t *node = NODE_ID_TO_PTR(mutex->head);
		  node->wait = 0;
		  
		  notify = ID_TO_EVENT(node->event);
		  node->next->prev = node->prev;
		  node->prev->next = node->next;
		  
		  if (mutex->head == NODE_PTR_TO_ID(node->next))
			   mutex->wait = 0;
		  else mutex->head = NODE_PTR_TO_ID(node->next);
	 }
	 else mutex->hold = 0;
	 spl_release(&mutex->lock);
	 intr_irq_restore();
	 if (notify != NULL)
		  proc_event_activate(notify);
}

void
semaphore_init(semaphore_t *semaphore, uint32_t count)
{
	 spl_init(&semaphore->lock);
	 semaphore->count = count;
	 semaphore->wait = 0;
}

int
semaphore_try_acquire(semaphore_t *semaphore)
{
	 int result;
	 intr_irq_save();
	 spl_acquire(&semaphore->lock);
	 if (semaphore->count > 0)
	 {
		  if (-- semaphore->count == 0)
			   semaphore->wait = 0;
		  result = 0;
	 }
	 else result = 1;
	 spl_release(&semaphore->lock);
	 intr_irq_restore();
	 return result;
}

int
semaphore_acquire(semaphore_t *semaphore, ips_node_t *node, proc_event_t event)
{
	 if (node == NULL)
	 {
		  ips_node_t node;
		  if (semaphore_acquire(semaphore, &node, event))
			   ips_wait(&node);

		  return 0;
	 }
	 else
	 {
		  intr_irq_save();
		  spl_acquire(&semaphore->lock);
		  
		  if (semaphore->count > 0)
		  {
			   if (-- semaphore->count == 0)
					semaphore->wait = 0;
		  
			   spl_release(&semaphore->lock);
			   intr_irq_restore();
			   
			   node->wait = 0;
		  
			   return 0;
		  }
		  else
		  {
			   node->event   = EVENT_TO_ID(event == NULL ? proc_self_dummy_event() : event);
			   node->ac_wait = 1;
			   node->wait    = 1;

			   if (semaphore->wait)
			   {
					node->next = NODE_ID_TO_PTR(semaphore->head);
					node->prev = node->next->prev;
					node->next->prev = node;
					node->prev->next = node;
			   }
			   else
			   {
					semaphore->wait = 1;
					node->next = node->prev = node;
					semaphore->head = NODE_PTR_TO_ID(node);
			   }

			   spl_release(&semaphore->lock);
			   intr_irq_restore();
			   
			   return 1;
		  }		  
	 }
}

void
semaphore_ac_break(semaphore_t *semaphore, ips_node_t *node)
{
	 intr_irq_save();
	 spl_acquire(&semaphore->lock);
	 if (node->wait)
	 {
		  node->next->prev = node->next;
		  node->prev->next = node->prev;
		  if (semaphore->head == NODE_PTR_TO_ID(node))
		  {
			   if (node->next == node)
					semaphore->wait = 0;
			   else semaphore->head = NODE_PTR_TO_ID(node->next);
		  }
	 }
	 spl_release(&semaphore->lock);
	 intr_irq_restore();
	 
	 node->ac_wait = 0;
}

int
semaphore_release(semaphore_t *semaphore)
{
	 int result = 0;
	 proc_event_t notify = NULL;
	 intr_irq_save();
	 spl_acquire(&semaphore->lock);
	 if (semaphore->wait)
	 {
		  ips_node_t *node = NODE_ID_TO_PTR(semaphore->head);
		  node->wait = 0;
		  
		  notify = ID_TO_EVENT(node->event);
		  node->next->prev = node->prev;
		  node->prev->next = node->next;
		  if (node->next == node)
			   semaphore->wait = 0;
		  else semaphore->head = NODE_PTR_TO_ID(node->next);

		  result = 1;
	 }
	 else
	 {
		  ++ semaphore->count;
	 }
	 spl_release(&semaphore->lock);
	 intr_irq_restore();

	 if (notify != NULL)
	 {
		  proc_event_activate(notify);
	 }

	 return result;
}

#define BQ_NOWAIT  0
#define BQ_WAITPUT 1
#define BQ_WAITGET 2

void
bqueue_init(bqueue_t *bq)
{
	 spl_init(&bq->lock);
	 bq->wait = BQ_NOWAIT;
}

int
bqueue_put(bqueue_t *bq, ips_node_t *node, proc_event_t event)
{
	 if (node == NULL)
	 {
		  ips_node_t node;
		  if (bqueue_put(bq, &node, event))
			   ips_wait(&node);
		  
		  return 0;
	 }
	 else
	 {
		  intr_irq_save();
		  spl_acquire(&bq->lock);
		  if (bq->wait == BQ_WAITPUT)
		  {
			   node = NODE_ID_TO_PTR(bq->head);
			   node->wait = 0;
		  
			   node->next->prev = node->prev;
			   node->prev->next = node->next;
			   if (node->next == node)
					bq->wait = BQ_NOWAIT;
			   else bq->head = NODE_PTR_TO_ID(node->next);

			   spl_release(&bq->lock);
			   intr_irq_restore();
			   
			   proc_event_activate(ID_TO_EVENT(node->event));

			   return 0;
		  }
		  else
		  {
			   node->event   = EVENT_TO_ID(event == NULL ? proc_self_dummy_event() : event);
			   node->ac_wait = 1;
			   node->wait    = 1;

			   if (bq->wait == BQ_WAITGET)
			   {
					node->next = NODE_ID_TO_PTR(bq->head);
					node->prev = node->next->prev;
					node->next->prev = node;
					node->prev->next = node;
			   }
			   else
			   {
					bq->wait = BQ_WAITGET;
					node->next = node->prev = node;
					bq->head = NODE_PTR_TO_ID(node);
			   }

			   spl_release(&bq->lock);
			   intr_irq_restore();
			   return 1;
		  }		  
	 }
}

int
bqueue_get(bqueue_t *bq, ips_node_t *node, proc_event_t event)
{
	 if (node == NULL)
	 {
		  ips_node_t node;
		  if (bqueue_get(bq, &node, event))
			   ips_wait(&node);
		  
		  return 0;
	 }
	 else
	 {
		  spl_acquire(&bq->lock);
		  if (bq->wait == BQ_WAITGET)
		  {
			   node = NODE_ID_TO_PTR(bq->head);
			   node->wait = 0;
		  
			   node->next->prev = node->prev;
			   node->prev->next = node->next;
		  
			   if (node->next == node)
					bq->wait = BQ_NOWAIT;
			   else bq->head = NODE_PTR_TO_ID(node->next);

			   spl_release(&bq->lock);
			   intr_irq_restore();
			   
			   proc_event_activate(ID_TO_EVENT(node->event));

			   return 0;
		  }
		  else
		  {
			   node->event   = EVENT_TO_ID(event == NULL ? proc_self_dummy_event() : event);
			   node->ac_wait = 1;
			   node->wait    = 1;

			   if (bq->wait == BQ_WAITPUT)
			   {
					node->next = NODE_ID_TO_PTR(bq->head);
					node->prev = node->next->prev;
					node->next->prev = node;
					node->prev->next = node;
			   }
			   else
			   {
					bq->wait = BQ_WAITPUT;
					node->next = node->prev = node;
					bq->head = NODE_PTR_TO_ID(node);
			   }

			   spl_release(&bq->lock);
			   intr_irq_restore();
		  
			   return 1;
		  }		  
	 }
}
